#include <Eigen/Dense>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <iostream>

int main()
{   
    double s = M_PI / 180.;
    
    double roll = -78 * s;
    double pitch = -10 * s;
    double yaw = 91 * s;

    Eigen::Quaterniond q_eigen(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) *
	Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) *
	Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()));

    Eigen::Matrix3d R_eigen(q_eigen);
    Eigen::Vector3d euler2 = R_eigen.eulerAngles(0,1,2) / s;
    Eigen::Vector3d euler3 = R_eigen.eulerAngles(2,1,0) / s;


    tf2::Quaternion q_tf2;
    q_tf2.setRPY(roll, pitch, yaw);

    tf2::Quaternion q_tf3;
    q_tf3.setEulerZYX(yaw, pitch, roll);

    double yaw1, pitch1, roll1;
    tf2::Matrix3x3(q_tf2).getRPY(roll1, pitch1, yaw1);

    double yaw2, pitch2, roll2;
    tf2::Matrix3x3(q_tf2).getEulerZYX(yaw2, pitch2, roll2);


    std::cout << "eigen:\n" << q_eigen.coeffs().transpose() << std::endl;

    
    std::cout << "\ntf2:\n" << q_tf2.x() << "  " << q_tf2.y() << "  " << q_tf2.z() << "  " << q_tf2.w() << std::endl;

    std::cout << "\ntf3:\n" << q_tf3.x() << "  " << q_tf3.y() << "  " << q_tf3.z() << "  " << q_tf3.w() << std::endl;

    std::cout << "\neigen2:\n" << euler2.transpose() << std::endl;
    std::cout << "\neigen3:\n" << euler3.transpose() << std::endl;

    std::cout << "\ntf2:\n" << roll1 / s << "  " << pitch1 / s << "  " << yaw1  / s<< std::endl;
    std::cout << "\ntf2:\n" << roll2 / s << "  " << pitch2 / s<< "  " << yaw2 / s << std::endl;


    return 0;

}